1. Field of the Invention
This invention relates generally to a system and method for identifying the curvature of a roadway using map database shape points and, more particularly, to a system and method for identifying roadway curvature in a semi-autonomous or autonomously driven vehicle using map database shape points and a multi-point polynomial curve fitting algorithm.
2. Discussion of the Related Art
The operation of modern vehicles is becoming more autonomous, i.e., vehicles are able to provide driving control with less and less driver intervention. Cruise control systems have been on vehicles for a number of years where the vehicle operator can set a particular speed of the vehicle, and the vehicle will maintain at that speed without the driver operating the throttle. Adaptive cruise control systems have been recently developed in the art where not only does the system maintain the set speed, but also will automatically slow the vehicle down in the event that a slower moving preceding vehicle is detected using various sensors, such as radar and cameras. Certain modern vehicles also provide autonomous parking where the vehicle will automatically provide the steering control for parking the vehicle. Some vehicle systems intervene if the driver makes harsh steering changes that may affect vehicle stability. Some vehicle systems attempt to maintain the vehicle near the center of a lane on the road. Further, fully autonomous vehicles have been demonstrated that can drive in simulated urban traffic up to 30 mph, observing all of the rules of the road.
As vehicle systems improve, they will become more autonomous with the goal being a complete autonomously driven vehicle. For example, future vehicles probably will employ autonomous systems for lane changing, passing, turns away from traffic, turns into traffic, etc. Smooth maneuvering and automated lane centering and lane changing control is important for driver and passenger comfort in autonomously driven vehicles. However, as a result of sensor and actuator latency, measured vehicle states may be different from actual vehicle states. This difference may cause improper path generation, which will affect lane changing harshness.
U.S. Pat. No. 8,170,739 issued May 1, 2012, titled, Path Generation Algorithm for Automated Lane Centering and Lane Changing Control System, assigned to the assignee of this application and herein incorporated by reference, discloses a system for providing path generation for automated lane center and/or lane changing purposes. The system employs one or more forward-looking cameras that detect lane markings in front of the vehicle for identifying a travel lane on which the vehicle is traveling. The road marking information detected by the cameras is used to determine a center lane of the vehicle that can be used to identify the curvature of the roadway, the heading angle of the vehicle, location of the vehicle, etc. A desired path generation processor receives the signals from the camera, vehicle state information and a steering angle of the vehicle, and a request for a vehicle lane change. The system also includes a path prediction processor that predicts the vehicle path based on the vehicle state information including vehicle longitudinal speed, vehicle lateral speed, vehicle yaw-rate and vehicle steering angle. The desired path information and the predicted path information are compared to generate an error signal that is sent to a lane change controller that provides a steering angle signal to turn the vehicle and reduce the error signal, where the path generation processor employs a fifth-order polynomial equation to determine the desired path of the vehicle based on the input signals.
The systems discussed above typically require knowledge of the roadway curvature to provide smooth vehicle path planning. It is known in the art to use a GPS receiver and associated map database on a vehicle for identifying the curvature of the roadway on which the vehicle is traveling. The GPS receiver identifies the location of the vehicle by latitude and longitude on the Earth and the map database uses that location to reconcile what roadway the vehicle is on and provides a number of shape points identifying the location of the roadway also in latitude and longitude around the vehicle. The available shape points in the map database are typically spaced apart depending on the curvature of the roadway, where the shape points are more closely spaced together for greater curved roads. The shape points can be connected by a line, and by fitting the line to a curve equation, the curvature of the roadway can be determined, where the curvature of the line travels through each of the shape points identified by database. However, because there are errors in the survey of the roadway, the map database shape points may not be exactly at the center of the roadway and may be somewhat off the roadway, which creates errors in the curve that is generated by the shape points.
Because the shape point locations have errors, the curvature of the line between the shape points may not accurately identify the proper radius of curvature of the road at any particular location. Because the outliers in the shape points in the map database are significant enough to cause the radius of curvature of the road to be significantly inaccurate, known techniques for identifying roadway curvature using map database shape points cannot be reliably employed.